/*
 * File:    Timer1.c
 * Project:	HobbyPilotFirmware (HPF)
 * Author:  Wouter van Verre
 *
 * Created on February 8, 2011, 10:35 PM
 */

#include "Header.h"

// Timer1 runs at 100Hz
void initTimer1(void)
{
    T1CONbits.TON = 0;		// Turn the Timer1 module off
    T1CONbits.TSIDL = 1;	// Stop when the uC enters idle mode
    T1CONbits.TGATE = 0;	// Disable gated accumulation mode
    T1CONbits.TCKPS = 0b11;	// Set prescaler to 256
    T1CONbits.TSYNC = 0;	// Don't synchronize the clock input
    T1CONbits.TCS = 0;		// Use internal clock as clock source

    PR1 = 575;          	// ~100Hz (99.96Hz) @ Fcy = 14,74Mhz

    T1CONbits.TON = 1;		// Turn the Timer1 module on
}

void __attribute__((__interrupt__, __no_auto_psv__)) _T1Interrupt (void)
{
	EnterInterrupt();

	if (Timer1FirstRun == true)
	{
		// Code to be run once
		initCircularBuffer(&UART1TXBuffer, 100);
		initCircularBuffer(&UART1RXBuffer, 25);

		T2CONbits.TON = 1;	// Turn on Timer2 for Output Compare

		Timer1FirstRun = false;
	}

	if (initCounter == 0)
	{
		if (Timer1Counter == 100)
		{
			// 1Hz loop
			
			Timer1Counter = 0;
		}
	
		if (Timer1Counter % 50 == 0) 
		{
			// 2Hz loop
		}
	
		if (Timer1Counter % 25 == 0) 
		{
			// 4Hz loop
		}
	
		if (Timer1Counter % 20 == 0)
		{
			// 5Hz loop
			toggleActivityLED();
	
			uint16 value;
			value = cpuLoadCycles;
			cpuLoadCycles = 0;
			cpuLoad = (value * 819200 * 5) / FCY;
			cpuLoadMax = (cpuLoad > cpuLoadMax) ? cpuLoad : cpuLoadMax;
			
			cpuStackUse = WREG15;
			GL_Send_CPULoadInfo();
		}
	
		if (Timer1Counter % 10 == 0)
		{
			// 10Hz loop
		}	
	
		if (Timer1Counter % 5 == 0)
		{
			// 20Hz loop
		}
	
		if (Timer1Counter % 4 == 0)
		{
			// 25Hz loop
		}
	
		if (Timer1Counter % 2 == 0)
		{
			// 50Hz loop
			GL_Send_RotationMatrix();
		}
	
		Timer1Counter++;
		
		// 100Hz loop
		readSensorChannels();	
		UpdateRotationMatrix();			
		RenormalizeRotationMatrix();	
		CancelDrift();			
	}
	else
	{
		readSensorChannels();
		if (initCounter == 200)
		{
			XRate.offset = XRate.value;
			YRate.offset = YRate.value;
			ZRate.offset = ZRate.value;
			XAccel.offset = XAccel.value;
			YAccel.offset = YAccel.value;
			ZAccel.offset = ZAccel.value;
		} 
		else
		{
			XRate.offset = (XRate.offset + XRate.value) >> 1;
			YRate.offset = (YRate.offset + YRate.value) >> 1;
			ZRate.offset = (ZRate.offset + ZRate.value) >> 1;
			XAccel.offset = (XAccel.offset + XAccel.value) >> 1;
			YAccel.offset = (YAccel.offset + YAccel.value) >> 1;
			ZAccel.offset = (ZAccel.offset + ZAccel.value) >> 1;
		}
	
		initCounter--;
		
		if (initCounter == 0)
		{
			ZAccel.offset += 248;
		}
	}

	IFS0bits.T1IF = 0;	// Clear the Timer1 interrupt flag
}

/**
void PrintFloat(union SplitFloat data)
{
	writeByteToCircularBuffer(&UART1TXBuffer, data.uint8.a);
	writeByteToCircularBuffer(&UART1TXBuffer, data.uint8.b);
	writeByteToCircularBuffer(&UART1TXBuffer, data.uint8.c);
	writeByteToCircularBuffer(&UART1TXBuffer, data.uint8.d);
	writeToUART1();
}
**/
